{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "85e0ca79-32de-4fb2-b92f-a9177d7d5652",
   "metadata": {},
   "source": [
    "# Sophus tutorials\n",
    "### Terminology\n",
    "* SO3: rotation group in 3D\n",
    "* SE3: rotation and translation group in 3D\n",
    "\n",
    "### Supported features\n",
    "#### SO3: Rotation group in 3D\n",
    "* Initialize from rotational vector, quaternion, rotation matrix\n",
    "* Convert to rotational vector, quaternion, rotation matrix\n",
    "* Multiplication with SO3 or 3D points\n",
    "* Operator [] for setting/getting items with index or slices\n",
    "* Inverse, copy, print, and len\n",
    "* Function vectorization\n",
    "\n",
    "#### SE3\n",
    "* Initialize from twist vector (rotation vector and translational vector), quaternion and translation, transformation matrix\n",
    "* Convert to twist vector, quaternion and translation, transformation matrix\n",
    "* Multiplication with SE3 or 3D points\n",
    "* Operator [] for setting/getting items with index or slices\n",
    "* Function vectorization\n",
    "* Inverse, copy, print, and len\n",
    "* Interpolate between two SE3\n",
    "* Iterative mean of a group of SE3 \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50dc0645-59ca-48d1-bb45-0fd32b8c0057",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sophus_pybind import interpolate, iterativeMean, SE3, SO3\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "09084939-7ee9-42fc-9cf1-41f4583c30dd",
   "metadata": {},
   "source": [
    "# SO3: Rotation group in 3D"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "00f04460-2843-4468-9a76-6a398a546c3c",
   "metadata": {},
   "source": [
    "### From/to rotation vector\n",
    "* rotation_vector: 3x1 or Nx3\n",
    "* The magnitude of the vector is the rotation angle in rad, the rotation direction is the norm of the rotation_vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c4a1e41a-eec7-4125-8432-6c0fe854c963",
   "metadata": {},
   "outputs": [],
   "source": [
    "rotation_vector = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 1]])\n",
    "so3vec_from_rotvec = SO3.exp(rotation_vector)\n",
    "print(f\"Initialize from rotation vector {rotation_vector}: \\n resulting log: \\n {so3vec_from_rotvec.log()}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "62e11068-bc23-4096-9e81-fc2624f28701",
   "metadata": {},
   "source": [
    "### From/to quaternion\n",
    "* Quaternion in put is represented as w, xyz\n",
    "  * Vectorized input is w_vec: Nx1 and xyz_vec: Nx3\n",
    "* Quaternion is the **Hamilton** convention"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "290c007b-59a3-4457-89e4-19327be0c274",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_w = 1; q_xyz = np.array([0,0,0])\n",
    "so3_from_quaternion = SO3.from_quat(q_w, q_xyz)\n",
    "print(f\"Initialize from quaternion [{q_w}, {q_xyz}]: \\n resulting quaternion: \\n {so3_from_quaternion.to_quat()}\")\n",
    "\n",
    "# Vectorized initialization\n",
    "q_w_vec = np.array([1,0])\n",
    "q_xyz_vec = np.array([[0,0,0], [0.0, 1.0, 0.0]])\n",
    "so3vec_from_quaterion = SO3.from_quat(q_w_vec, q_xyz_vec)\n",
    "for i in range(0,len(q_w_vec)):\n",
    "    print(f\"Initialize from quaternion [{q_w_vec[i]}, {q_xyz_vec[i]}]: \\n resulting quaternion: \\n{so3vec_from_quaterion.to_quat()[i]}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5a00f27e-882c-48a7-a1dc-56268d79df0a",
   "metadata": {},
   "source": [
    "### From/to rotation matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "20820ff6-2c75-4430-8727-02173e3a9e1c",
   "metadata": {},
   "outputs": [],
   "source": [
    "rotation_matrix = [[-1,0,0], [0, 1,0 ], [0,0,-1]]\n",
    "so3_from_matrix = SO3.from_matrix(rotation_matrix)\n",
    "print(f\"Initialize from matrix {rotation_matrix}: \\n resulting matrix: \\n{so3_from_matrix.to_matrix()}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "33006cb6-5b3f-4f8b-8cb0-f2d5e1e56792",
   "metadata": {},
   "source": [
    "### Multiplication\n",
    "* Matrix multiplication uses **@** operator\n",
    "* Supports\n",
    "  * Multiple SO3 @ Single SO3\n",
    "  * Single SO3 @ Multiple SO3\n",
    "  * Single SO3 @ Multiple 3D points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e75e570-c99d-47d3-8245-4b79df8aa36a",
   "metadata": {},
   "outputs": [],
   "source": [
    "so3_result_1 = so3_from_matrix @ so3vec_from_rotvec[0]\n",
    "so3_result_2 = so3_from_matrix[0] @ so3vec_from_rotvec\n",
    "\n",
    "original_points = np.random.rand(3,10)\n",
    "points_from_a_single_so3 = so3vec_from_rotvec[0] @ original_points"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7fab0a78-6a5e-4ab0-b9fb-905829b92b8c",
   "metadata": {},
   "source": [
    "### MISC\n",
    "* Inverse\n",
    "* copy, str, len, repr\n",
    "* Operator [] to get/set from indices and slice"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "012c03c4-3ef6-4515-a08a-3e2b720d3590",
   "metadata": {},
   "outputs": [],
   "source": [
    "so3_inverse = so3vec_from_rotvec.inverse()\n",
    "so3_slice = so3_inverse[1:3]\n",
    "\n",
    "import copy\n",
    "print(f\"A copy of the SO3 is {copy.copy(so3_inverse)}\\n\")\n",
    "print(f\"The str of the SO3 is {so3_inverse}\\n\")\n",
    "print(f\"The len of the SO3 is {len(so3_inverse)}\\n\")\n",
    "print(f\"The repr of the SO3 is {repr(so3_inverse)}\\n\")\n",
    "print(f\"A SO3 slice is {so3_slice}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "14a0c1a3-9db5-4c17-98fb-9dd200c9d0a6",
   "metadata": {},
   "source": [
    "# SE3: transformation group in 3D"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1075eb08-c604-44a3-aa89-ad055a11874d",
   "metadata": {},
   "source": [
    "### From/to translational vector and rotation vector(twist)\n",
    "* SE3.exp(translational_part, rotation_vector)\n",
    "* [translational_part, rotation_vector] = SE3.log()\n",
    "  \n",
    "NOTE: translational_vector is not translation in SE3, it is off by a left jacobian matrix (ref: https://github.com/strasdat/Sophus/blob/main-1.x/sophus/se3.hpp#L859-L860)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8383f639-cbc6-49ac-b715-141db0f5f9cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "translational_part = [[1,2,3],[15,2,9],[10,4,3],[3,2,3]]\n",
    "rotation_vector = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 1]]\n",
    "se3_from_rotvec_and_translational = SE3.exp(translational_part, rotation_vector)\n",
    "twist = se3_from_rotvec_and_translational.log()\n",
    "print(f\"Initialize from rotation vector {rotation_vector} and translational_part {translational_part}: \\n \\\n",
    "resulting twist: \\n{twist}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "297730dc-0665-44f3-9596-a4886f6fc37b",
   "metadata": {},
   "source": [
    "### From/to quaternion and translation\n",
    "* SE3.from_quat_and_translation(w, xyz, translation)\n",
    "* [w, xyz, translation] = SE3.to_quat_and_translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3bf0df5e-718d-40b5-b0c0-7e55b8d549ea",
   "metadata": {},
   "outputs": [],
   "source": [
    "translation_vector = [[1,2,3],[15,2,9],[10,4,3],[3,2,3]]\n",
    "w_vec = [1,0,0,1]\n",
    "xyz_vec = [[0,0,0], [0,1,0], [0,0,1], [0,0,0]]\n",
    "se3 = SE3.from_quat_and_translation(w_vec, xyz_vec, translation_vector)\n",
    "quaternion_and_translation = se3.to_quat_and_translation()\n",
    "print(f\"Initialize from quaternion w {w_vec} and xyz {xyz_vec} \\nwith translational_part {translational_part}: \\n\")\n",
    "print(f\"resulting quat and translation output : \\n{quaternion_and_translation}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b6fc4e56-b744-44a1-a0a5-05149199ebfb",
   "metadata": {},
   "source": [
    "### From/to transformation matrix of size 3x4 or 4x4\n",
    "* SE3.from_matrix(matrix4x4)\n",
    "* SE3.from_matrix3x4(matrix3x4)\n",
    "* matrix4x4 = SE3.to_matrix()\n",
    "* matrix3x4 = SE3.to_matrix3x4()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0fab990-2d27-46bb-a836-5c92c4c089a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "transformation_matrix = np.array([[[ 1.,  0.,  0.,  1.],\n",
    "        [ 0.,  1.,  0.,  2.],\n",
    "        [ 0.,  0.,  1.,  3.],\n",
    "        [ 0.,  0.,  0.,  1.]],\n",
    "\n",
    "       [[-1.,  0.,  0., 15.],\n",
    "        [ 0.,  1.,  0.,  2.],\n",
    "        [ 0.,  0., -1.,  9.],\n",
    "        [ 0.,  0.,  0.,  1.]],\n",
    "\n",
    "       [[-1.,  0.,  0., 10.],\n",
    "        [ 0., -1.,  0.,  4.],\n",
    "        [ 0.,  0.,  1.,  3.],\n",
    "        [ 0.,  0.,  0.,  1.]],\n",
    "\n",
    "       [[ 1.,  0.,  0.,  3.],\n",
    "        [ 0.,  1.,  0.,  2.],\n",
    "        [ 0.,  0.,  1.,  3.],\n",
    "        [ 0.,  0.,  0.,  1.]]])\n",
    "se3_from_matrix = SE3.from_matrix(transformation_matrix)\n",
    "se3_from_matrix3x4 = SE3.from_matrix3x4(transformation_matrix[:, 0:3,:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7a8d25b7-177b-4eda-b6ae-f904a931f696",
   "metadata": {},
   "source": [
    "### Multiplication\n",
    "* Matrix multiplication uses **@** operator\n",
    "* Supports\n",
    "  * Multiple SE3 @ Single SE3\n",
    "  * Single SE3 @ Multiple SE3\n",
    "  * Single SE3 @ Multiple 3D points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "930f1747-f773-464e-bcd4-d69b7579fdcf",
   "metadata": {},
   "outputs": [],
   "source": [
    "se3_result_1 = se3_from_matrix @ se3_from_rotvec_and_translational[0]\n",
    "se3_result_2 = se3_from_matrix[0] @ se3_from_rotvec_and_translational\n",
    "\n",
    "original_points = np.random.rand(3,10)\n",
    "points_from_a_single_se3 = se3_from_matrix[0] @ original_points"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c5fdcac8-499a-425d-8f06-f1a8ffd0b7fc",
   "metadata": {},
   "source": [
    "### MISC\n",
    "* Inverse\n",
    "* copy, str, len, repr\n",
    "* Operator [] to get/set from indices and slice"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "306380b4-ec6c-4672-aa28-e396e9cd8eac",
   "metadata": {},
   "outputs": [],
   "source": [
    "se3_inverse = se3_from_matrix.inverse()\n",
    "se3_slice = se3_inverse[0:2]\n",
    "\n",
    "print(f\"A copy of the SE3 is {copy.copy(se3_inverse)}\\n\")\n",
    "print(f\"The str of the SE3 is {se3_inverse}\\n\")\n",
    "print(f\"The len of the SE3 is {len(se3_inverse)}\\n\")\n",
    "print(f\"The repr of the SE3 is {repr(se3_inverse)}\\n\")\n",
    "print(f\"A SE3 slice is {se3_slice}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d61cf4ed-8abe-4aec-a11a-377c4ae5d3c6",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
